/********** System Includes ***************************************************/

#include <queue>

/********** Library Includes **************************************************/

#include <opencv/cv.h>
#include <opencv/highgui.h>

/********** Local Includes ****************************************************/

#include "celldecompastar.h"

#include "astarheuristics.h"
#include "common.h"

/********** Namespaces ********************************************************/

using namespace cv;
using namespace std;

/********** Public Methods ****************************************************/

CellDecompAStar::CellDecompAStar(const Mat &grid, AStarHeuristic *heuristic) :
	m_heuristic(heuristic)
{
	static bool gotOne = false;
	
	for (int row = 0; row < grid.size().height; row++) {
		m_nodes.push_back(NodeVector());
		
		for (int col = 0; col < grid.size().width; col++) {
			if (grid.at<float>(row, col) > .9) {
				m_nodes[row].push_back(Node(Point(col, row)));
				
				if (grid.at<float>(row, col) != 1 && !gotOne) {
					gotOne = true;
				}
			} else {
				m_nodes[row].push_back(Node());
			}
		}
	}
	
	/*
	for (int row = 0; row < grid.size().height; row++) {
		for (int col = 0; col < grid.size().width; col++) {
			cout << (m_nodes[row][col].valid ? "o" : "x") << " ";
		}
		
		cout << endl;
	}
	*/
	
	int height = m_nodes.size();
	
	for (int row = 0; row < height; row++) {
		int width = m_nodes[row].size();
		
		for (int col = 0; col < width; col++) {
			if (row > 0 && m_nodes[row - 1][col].valid) {
				m_nodes[row][col].neighbors.push_back(&m_nodes[row-1][col]);
			}
			
			if (row < height - 1 && m_nodes[row + 1][col].valid) {
				m_nodes[row][col].neighbors.push_back(&m_nodes[row+1][col]);
			}
			
			if (col > 0 && m_nodes[row][col - 1].valid) {
				m_nodes[row][col].neighbors.push_back(&m_nodes[row][col - 1]);
			}
			
			if (col < width - 1 && m_nodes[row][col + 1].valid) {
				m_nodes[row][col].neighbors.push_back(&m_nodes[row][col + 1]);
			}
			
			if (row > 0 && col > 0 && m_nodes[row - 1][col - 1].valid) {
				m_nodes[row][col].neighbors.push_back(&m_nodes[row - 1][col - 1]);
			}
			
			if (row > 0 && col < width - 1 && m_nodes[row - 1][col + 1].valid) {
				m_nodes[row][col].neighbors.push_back(&m_nodes[row - 1][col + 1]);
			}
			
			if (row < height - 1 && col > 0 && m_nodes[row + 1][col - 1].valid) {
				m_nodes[row][col].neighbors.push_back(&m_nodes[row + 1][col - 1]);
			}
			
			if (row < height - 1 && col < width - 1 && m_nodes[row + 1][col + 1].valid) {
				m_nodes[row][col].neighbors.push_back(&m_nodes[row + 1][col + 1]);
			}
		}
	}
}

vector<Point> CellDecompAStar::plan(Point start, Point goal) {
	if (!m_heuristic) {
		ERROR("Can't do anything without a heuristic");
		return vector<Point>();
	}
	
	cout << "Preparing to plan" << endl;
	cout << "Start: (" << start.x << ", " << start.y << ")" << endl;
	cout << "Goal:  (" << goal.x << ", " << goal.y << ")" << endl;
	
	if (!isValid(start)) {
		ERROR("Starting point must be a valid node");
		return vector<Point>();
	}
	
	if (!isValid(goal)) {
		ERROR("Goal point must be a valid node");
		return vector<Point>();
	}
	
	priority_queue<Path> paths;
	paths.push(Path(&m_nodes[start.y][start.x], 0, 0));
	int opened = 0;
	
	while (!paths.empty()) {
		Path top = paths.top();
		paths.pop();
		
		Node *last = *(top.nodes.end() - 1);
		
		if (last->location == goal) {
			cout << "Nodes opened: " << opened << endl;
			cout << "Plan's heuristic value: "
				<< m_heuristic->expected(top, goal) << endl;
			return toPoints(top.nodes);
		}
			
		if (!last->visited) {
			last->visited = true;
			opened++;
			
			for (vector<Node *>::const_iterator it = last->neighbors.begin();
				it != last->neighbors.end(); ++it)
			{
				if (!(*it)->visited) {
					paths.push(Path(top, *it, 1,
						m_heuristic->expected(top, goal)));
				}
			}
		}
	}
	
	WARNING("No path to the goal!");
	return vector<Point>();
}

/********** Private Methods ***************************************************/

bool CellDecompAStar::isValid(const Point &p) const {
	size_t row = static_cast<size_t>(p.y);
	size_t col = static_cast<size_t>(p.x);
	
	return row >= 0 && col >= 0 && row < m_nodes.size()
		&& col < m_nodes[row].size() && m_nodes[row][col].valid;
}

vector<Point> CellDecompAStar::toPoints(const vector<Node *> &path) const {
	vector<Point> points;
	
	for (vector<Node *>::const_iterator it = path.begin(); it != path.end();
		++it)
	{
		points.push_back((*it)->location);
	}
	
	/*
	 * The idea here will be to iterate over the points vector looking for line
	 * segments.  start will be the index of the first point in a segment, and
	 * end will be the last point in a segment.  Every segment will contain at
	 * least two points, so ( points[i], points[i + 1] ) is the shortest
	 * possible segment (provided that there are at least two points in the
	 * vector.
	 *
	 * To compress the vector, this will fix start and increment end.  As long
	 * as a turn is not found, the redundent point will be removed.  When a turn
	 * is found, the last point in the old segment becomes the first point in
	 * the new segment.
	 */
	size_t start = 0;
	size_t end = 1;
	
	while (end < points.size() - 1) {
		// The direction the robot is driving the current segment
		vector<double> current = unit(points[start], points[end]);
		
		// The direction the robot is driving in the next leg
		vector<double> next = unit(points[end], points[end + 1]);
		
		if (current == next) { // The same direction
			points.erase(points.begin() + end);
		} else { // A turn
			start = end;
			end++;
		}
	}
	
	return points;
}

vector<double> CellDecompAStar::unit(Point one, Point two) const {
	Point diff = two - one;
	double norm = sqrt(diff.x * diff.x + diff.y * diff.y);
	
	vector<double> unit;
	unit.push_back(diff.x / norm);
	unit.push_back(diff.y / norm);
	return unit;
}

/********** Program Entry *****************************************************/

Mat toFloatingPoint(const Mat &integer) {
	Mat floating(integer.size(), CV_32FC1);
	
	for (int row = 0; row < integer.size().height; row++) {
		for (int col = 0; col < integer.size().width; col++) {
			if (integer.at<Vec3b>(row, col)[0] > 100) {
				floating.at<float>(row, col) = 1;
			}
		}
	}
	
	return floating;
}

/*
int main(int, char **) {
	Mat grid = toFloatingPoint(imread("planning_input/real.jpg"));
	CellDecompAStar astar(grid);
	vector<Point> plan = astar.plan(Point(1, 12), Point(28, 10));
	
	int multiplier = 20;
	Mat display(grid.size() * multiplier, CV_8UC3);
	display = CV_RGB(255, 255, 255);
	
	for (int row = 0; row < grid.size().height; row++) {
		for (int col = 0; col < grid.size().width; col++) {
			if (grid.at<float>(row, col) < .5) {
				rectangle(display, Point(col * multiplier, row * multiplier),
					Point((col + 1) * multiplier, (row + 1) * multiplier),
					CV_RGB(0, 0, 0), CV_FILLED);
			}
		}
	}
	
	for (vector<Point>::iterator it = plan.begin(); it != plan.end(); ++it) {
		rectangle(display, *it * multiplier, Point(it->x + 1, it->y + 1) * multiplier,
			CV_RGB(255, 0, 0), CV_FILLED);
	}
	
	SHOW(display);
	
	while (waitKey(300) != 'q');
}
*/